import math
from ..common.data_bus import DataBus
from ..mathq.ramp_trajectory import RampTrajectory

class JoyStickInterpreter:
    def __init__(self, dt):
        self.dt = dt
        self.thetaZ = 0
        self.vxLGen = RampTrajectory()
        self.vyLGen = RampTrajectory()
        self.wzLGen = RampTrajectory()
        self.thetazGen = RampTrajectory()
        ## generated velocity in x and y direction w.r.t world frame
        self.vx_W = 0.0
        self.vy_W = 0.0 
        ##generated position in x and y direction w.r.t world frame
        self.px_W = 0.0
        self.py_W = 0.0
        ## generated linear velocity in x and y direction, angular velocity in z direction, w.r.t
        self.vx_L = 0.0
        self.vy_L = 0.0
        self.wz_L = 0.0

    def setVxDesLPara(self, vxDesLIn, timeToReach):
        self.vxLGen.setPara(vxDesLIn, timeToReach)

    def setVyDesLPara(self, vyDesLIn, timeToReach):
        self.vyLGen.setPara(vyDesLIn, timeToReach)

    def setWzDesLPara(self, wzDesLIn, timeToReach):
        self.wzLGen.setPara(wzDesLIn, timeToReach)

    def setIniPos(self, posX, posY, thetaZ):
        self.px_W = posX
        self.py_W = posY
        self.thetaZ = thetaZ

    def step(self):
        self.vx_L = self.vxLGen.step()
        self.vy_L = self.vyLGen.step()
        self.wz_L = self.wzLGen.step()
        self.thetaZ += self.wz_L * self.dt
        self.vx_W = math.cos(self.thetaZ) * self.vx_L - math.sin(self.thetaZ) * self.vy_L
        self.vy_W = math.sin(self.thetaZ) * self.vx_L + math.cos(self.thetaZ) * self.vy_L
        self.px_W += self.vx_W * self.dt
        self.py_W += self.vy_W * self.dt

    def dataBusWrite(self, dataBus):
        dataBus.js_pos_des[0] = self.px_W
        dataBus.js_pos_des[1] = self.py_W
        dataBus.js_vel_des[0] = self.vx_W
        dataBus.js_vel_des[1] = self.vy_W
        dataBus.js_eul_des[2] = self.thetaZ
        dataBus.js_omega_des[2] = self.wz_L

    def reset(self):
        self.vxLGen.resetOut(0)
        self.vyLGen.resetOut(0)
        self.wzLGen.resetOut(0)
        self.vx_L = 0
        self.vy_L = 0
        self.wz_L = 0
        self.thetaZ = 0